#include "hexacontrol.h"

HexaControl::HexaControl(QWidget *parent, Qt::WFlags flags)
    : QMainWindow(parent, flags)
{

   vGyro.x = 0;
   vGyro.y = 0;
   vGyro.z = 0;

   timerGL = new QTimer(this);
   timerCOM = new QTimer(this);
   timerCOM->stop();
   timerGL->stop();
   bCheckCom = false;
   setup(this);
   
}

HexaControl::~HexaControl()
{
  delete timerGL;
  delete timerCOM;
}

void HexaControl::updateGL(){
  glOpenGL->updateGL();

}

#include "staticui.h"


void HexaControl::addSetup(QMainWindow *HexaControlClass){

  glOpenGL = new GLWidget( &vGyro, &vAcc, &vMag,  this);
  glOpenGL->setObjectName(QString::fromUtf8("glOpenGL"));
  glOpenGL->setGeometry(QRect(10, 10, 600, 500));
  pbOpenComPort = new QPushButton(this);
  pbOpenComPort->setObjectName(QString::fromUtf8("pbOpenComPort"));
  pbOpenComPort->setGeometry(QRect(440, 550, 75, 23));
  pbCloseComPort = new QPushButton(this);
  pbCloseComPort->setObjectName(QString::fromUtf8("pbCloseComPort"));
  pbCloseComPort->setGeometry(QRect(530, 550, 75, 23));
  pbOpenComPort->setText(QApplication::translate("HexaControlClass", "Open", 0, QApplication::UnicodeUTF8));
  pbCloseComPort->setText(QApplication::translate("HexaControlClass", "Close", 0, QApplication::UnicodeUTF8));

  QObject::connect(pbOpenComPort, SIGNAL(clicked()), this, SLOT(openPort()));
  connect(pbCloseComPort, SIGNAL(clicked()), this, SLOT(closePort()));

  QMetaObject::connectSlotsByName(HexaControlClass);

  QObject::connect(timerGL, SIGNAL(timeout()), this, SLOT(updateGL()));
  timerGL->start(10);
  connect(timerCOM, SIGNAL(timeout()), this, SLOT(readPort()));
};

void HexaControl::openPort(){
  //if(cComPort-Open(sBComPort->value(),19200))
  tstring commPortName(TEXT("COM1"));
  bValidPort = false;
  cComPort = new Serial(commPortName, bValidPort, 57600);
  if (bValidPort == false){
    delete cComPort;
    timerCOM->stop();
    //todo:ausgabe das port nicht offen
  }else
    timerCOM->start(10);

};

void HexaControl::closePort(){
  if( bValidPort == true)
    delete cComPort;
  timerCOM->stop();
  //emit updatePort();
};

void HexaControl::readPort(){
  if (bCheckCom == false && bValidPort == true){
    bCheckCom = true;
    //if(cComPort.IsOpened()){
    char buffer[300];


    for(int i = 0; i < 300;i++)
      buffer[i] = 0;
    char *pbuffer = &buffer[0];
    //cComPort->read(pbuffer, 1000, true);
    while(cComPort->read(pbuffer, 1, false)){
      if(*pbuffer == 'A'){
        //cComPort->flush();
        break;
      }
      
      pbuffer++;
    }   

    string s(buffer);
    if(s.compare("") && (s.length() > 3)){
      int checksum = 0;
      if(s.at((size_t) 2) == '$'){
        for(int i = 3 ; i < s.size() ; i++){
          if(s.at((size_t) i) == ';'){
            checksum++;
          }else if (s.at((size_t) i) == 'A'){
            break;
          }
        }
      }
      if (checksum == 18){

        if((buffer[2] == 36 /*$ */) && (buffer[3] == 1 + 48)){

          int counter = 0;

          vGyro.x = 0;
          vGyro.y = 0;
          vGyro.z = 0;

          bool negative[3] = {false, false, false};

          for(int i = 4; i < 100 ; i++){    
            if(s.at((size_t) i) == 36 /*$ */)
              break;
            if(s.at((size_t) i) == ';'){
              counter++;
              continue;
            }else if(counter == 3){
              if(s.at((size_t) i) == '-'){
                negative[0] = true;
                continue;
              }
              vGyro.x = vGyro.x * 10 + (int)(s.at((size_t) i) - 48); 
            }else if(counter == 4){
              if(s.at((size_t) i) == '-'){
                negative[1] = true;
                continue;
              }
              vGyro.y = vGyro.y * 10 + (int)(s.at((size_t) i) - 48); 
            }else if(counter == 5){
              if(s.at((size_t) i) == '-'){
                negative[2] = true;
                continue;
              }
              vGyro.z = vGyro.z * 10 + (int)(s.at((size_t) i) - 48); 
            }else if(s.at((size_t) i) == 36 /* $ */){
              break;
            }
          }
          if(negative[0] == true)
            vGyro.x *= -1;
          if(negative[1] == true)
            vGyro.y *= -1;
          if(negative[2] == true)
            vGyro.z *= -1;
          pBGyroX->setValue(vGyro.x);
          pBGyroY->setValue(vGyro.y);
          pBGyroZ->setValue(vGyro.z);
        }
        //if((buffer[0] = 36 /*$ */) && (buffer[1] == 2 + 48)){

        //  int counter = 0;

        //  vAcc.x = 0;
        //  vAcc.y = 0;
        //  vAcc.z = 0;

        //  bool negative[3] = {false, false, false};

        //  for(int i = 0; i < 100 ; i++){    

        //    if(s.at((size_t) i) == ';'){
        //      counter++;
        //      continue;
        //    }
        //    if(counter == 3){
        //      if(s.at((size_t) i) == '-'){
        //        negative[0] = true;
        //        continue;
        //      }
        //      vAcc.x = vAcc.x * 10 + (int)(s.at((size_t) i) - 48); 
        //    }
        //    if(counter == 4){
        //      if(s.at((size_t) i) == '-'){
        //        negative[1] = true;
        //        continue;
        //      }
        //      vAcc.y = vAcc.y * 10 + (int)(s.at((size_t) i) - 48); 
        //    }
        //    if(counter == 5){
        //      if(s.at((size_t) i) == '-'){
        //        negative[2] = true;
        //        continue;
        //      }
        //      vAcc.z = vAcc.z * 10 + (int)(s.at((size_t) i) - 48); 
        //    }

        //    if(s.at((size_t) i) == 10){
        //      if(negative[0] == true)
        //        vAcc.x *= -1;
        //      if(negative[1] == true)
        //        vAcc.y *= -1;
        //      if(negative[2] == true)
        //        vAcc.z *= -1;
        //      break;
        //    }
        //  }
        //}
        //if((buffer[0] = 36 /*$ */) && (buffer[1] == 3 + 48)){

        //  int counter = 0;

        //  vMag.x = 0;
        //  vMag.y = 0;
        //  vMag.z = 0;

        //  bool negative[3] = {false, false, false};

        //  for(int i = 0; i < 100 ; i++){    

        //    if(s.at((size_t) i) == ';'){
        //      counter++;
        //      continue;
        //    }
        //    if(counter == 3){
        //      if(s.at((size_t) i) == '-'){
        //        negative[0] = true;
        //        continue;
        //      }
        //      vMag.x = vMag.x * 10 + (int)(s.at((size_t) i) - 48); 
        //    }
        //    if(counter == 4){
        //      if(s.at((size_t) i) == '-'){
        //        negative[1] = true;
        //        continue;
        //      }
        //      vMag.y = vMag.y * 10 + (int)(s.at((size_t) i) - 48); 
        //    }
        //    if(counter == 5){
        //      if(s.at((size_t) i) == '-'){
        //        negative[2] = true;
        //        continue;
        //      }
        //      vMag.z = vMag.z * 10 + (int)(s.at((size_t) i) - 48); 
        //    }

        //    if(s.at((size_t) i) == 10){
        //      if(negative[0] == true)
        //        vMag.x *= -1;
        //      if(negative[1] == true)
        //        vMag.y *= -1;
        //      if(negative[2] == true)
        //        vMag.z *= -1;
        //      break;
        //    }
        //  }
        //}
      }else{
        //readPort();
      }
    }
    bCheckCom = false;
  }

};